import cv2 as cv
import numpy as np
aruco_dict = cv.aruco.getPredefinedDictionary(cv.aruco.DICT_6X6_250)
detect_param = cv.aruco.DetectorParameters()

detector= cv.aruco.ArucoDetector(aruco_dict, detect_param)

# cap = cv.VideoCapture('imgs/1.mkv')
cap = cv.VideoCapture(0)
camera_matrix = [[369, 0, 640], [0, 369, 360], [0, 0, 1]]
dist_coffe = [0.0, 0.0, 0.0, 0.0, 0.0]
aruco_length = 0.6

while cap.isOpened():
    ret, frame = cap.read()
    (corners, ids, rejected) = detector.detectMarkers(frame)
    cv.aruco.drawDetectedMarkers(frame, corners, ids)
    (rvec, tvec, obj_points) = cv.aruco.estimatePoseSingleMarkers(corners, aruco_length, np.array(camera_matrix), np.array(dist_coffe))
    print(tvec)
    cv.imshow('img', frame)
    cv.waitKey(10)